
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_yaw_controller.c
  * @author     baiyang
  * @date       2023-6-30
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mp_yaw_controller.h"

#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void yctrl_assign_param(mp_yaw_controller_t yaw_ctrl);
/*----------------------------------variable----------------------------------*/
/* define parameters */
static param_t var_info[] = {
    // @Param: 2SRV_SLIP
    // @DisplayName: Sideslip control gain
    // @Description: Gain from lateral acceleration to demanded yaw rate for aircraft with enough fuselage area to detect lateral acceleration and sideslips. Do not enable for flying wings and gliders. Actively coordinates flight more than just yaw damping. Set after YAW2SRV_DAMP and YAW2SRV_INT are tuned.
    // @Range: 0 4
    // @Increment: 0.25
    // @User: Advanced
    PARAM_DEFINE_FLOAT(YAW2SRV_SLIP,0),

    // @Param: 2SRV_INT
    // @DisplayName: Sideslip control integrator
    // @Description: Integral gain from lateral acceleration error. Effectively trims rudder to eliminate long-term sideslip.
    // @Range: 0 2
    // @Increment: 0.25
    // @User: Advanced
    PARAM_DEFINE_FLOAT(YAW2SRV_INT,0),

    // @Param: 2SRV_DAMP
    // @DisplayName: Yaw damping
    // @Description: Gain from yaw rate to rudder. Most effective at yaw damping and should be tuned after KFF_RDDRMIX. Also disables YAW2SRV_INT if set to 0.
    // @Range: 0 2
    // @Increment: 0.25
    // @User: Advanced
    PARAM_DEFINE_FLOAT(YAW2SRV_DAMP,0),

    // @Param: 2SRV_RLL
    // @DisplayName: Yaw coordination gain
    // @Description: Gain to the yaw rate required to keep it consistent with the turn rate in a coordinated turn. Corrects for yaw tendencies after the turn is established. Increase yaw into the turn by raising. Increase yaw out of the turn by decreasing. Values outside of 0.9-1.1 range indicate airspeed calibration problems.
    // @Range: 0.8 1.2
    // @Increment: 0.05
    // @User: Advanced
    PARAM_DEFINE_FLOAT(YAW2SRV_RLL,1.0f),

    // @Param: 2SRV_IMAX
    // @DisplayName: Integrator limit
    // @Description: Limit of yaw integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 1500 allows trim of up to 1/3 of servo travel range.
    // @Range: 0 4500
    // @Increment: 1
    // @User: Advanced
    PARAM_DEFINE_INT16(YAW2SRV_IMAX,1500),

    // @Param: _RATE_ENABLE
    // @DisplayName: Yaw rate enable
    // @Description: Enable yaw rate controller for aerobatic flight
    // @Values: 0:Disable,1:Enable
    // @User: Advanced
    PARAM_DEFINE_INT8(YAW_RATE_ENABLE,0),

    PARAM_DEFINE_FLOAT(YAW_RATE_P,0.04f),
    PARAM_DEFINE_FLOAT(YAW_RATE_I,0.15f),
    PARAM_DEFINE_FLOAT(YAW_RATE_D,0),
    PARAM_DEFINE_FLOAT(YAW_RATE_FF,0.15f),
    PARAM_DEFINE_FLOAT(YAW_RATE_IMAX,0.666f),
    PARAM_DEFINE_FLOAT(YAW_RATE_FLTT,3.0f),
    PARAM_DEFINE_FLOAT(YAW_RATE_FLTE,0.0f),
    PARAM_DEFINE_FLOAT(YAW_RATE_FLTD,12.0f),
    PARAM_DEFINE_FLOAT(YAW_RATE_SMAX,150),
};

PARAM_GROUP_DEFINE(YAW_CTRL, var_info);

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void yctrl_ctor(mp_yaw_controller_t yaw_ctrl, mb_fixed_wing_const_t parms, float dt)
{
    yaw_ctrl->aparm = parms;
    yaw_ctrl->_dt = dt;

    yaw_ctrl->_pid_info.target = 0;
    yaw_ctrl->_pid_info.FF = 0;
    yaw_ctrl->_pid_info.P = 0;

    pid_ctrl_ctor2(&yaw_ctrl->rate_pid, 0.04f, 0.15f, 0, 0.15f, 0.666f, 3, 0, 12, 150, 1, dt);
    pid_ctrl_set_slew_limit_scale(&yaw_ctrl->rate_pid, 45);

    yctrl_assign_param(yaw_ctrl);
}

// get actuator output for direct rate control
// desired_rate is in deg/sec. scaler is the surface speed scaler
int32_t yctrl_get_servo_out(mp_yaw_controller_t yaw_ctrl, float scaler, bool disable_integrator)
{
    uint32_t tnow = time_millis();
    uint32_t dt = tnow - yaw_ctrl->_last_t;
    if (yaw_ctrl->_last_t == 0 || dt > 1000) {
        dt = 0;
        yaw_ctrl->_pid_info.I = 0;
    }
    yaw_ctrl->_last_t = tnow;


    int16_t aspd_min = yaw_ctrl->aparm->airspeed_min;
    if (aspd_min < 1) {
        aspd_min = 1;
    }

    float delta_time = (float) dt * 0.001f;

    ahrs_view *_ahrs = get_ahrs_view();

    // Calculate yaw rate required to keep up with a constant height coordinated turn
    float aspeed;
    float rate_offset;
    float bank_angle = _ahrs->roll;
    // limit bank angle between +- 80 deg if right way up
    if (fabsf(bank_angle) < 1.5707964f)	{
        bank_angle = math_constrain_float(bank_angle,-1.3962634f,1.3962634f);
    }

    if (!ahrs_airspeed_estimate(_ahrs, &aspeed)) {
        // If no airspeed available use average of min and max
        aspeed = 0.5f*(aspd_min + yaw_ctrl->aparm->airspeed_max);
    }
    rate_offset = (GRAVITY_MSS / MAX(aspeed, (float)aspd_min)) * sinf(bank_angle) * yaw_ctrl->_K_FF;

    // Get body rate vector (radians/sec)
    float omega_z = _ahrs->gyr.z;

    // Get the accln vector (m/s^2)
    float accel_y = _ahrs->acc.y;

    // subtract current bias estimate from EKF
    const Vector3f_t *abias = ahrs_get_accel_bias(_ahrs);
    accel_y -= abias->y;

    // Subtract the steady turn component of rate from the measured rate
    // to calculate the rate relative to the turn requirement in degrees/sec
    float rate_hp_in = ToDeg(omega_z - rate_offset);

    // Apply a high-pass filter to the rate to washout any steady state error
    // due to bias errors in rate_offset
    // Use a cut-off frequency of omega = 0.2 rad/sec
    // Could make this adjustable by replacing 0.9960080 with (1 - omega * dt)
    float rate_hp_out = 0.9960080f * yaw_ctrl->_last_rate_hp_out + rate_hp_in - yaw_ctrl->_last_rate_hp_in;
    yaw_ctrl->_last_rate_hp_out = rate_hp_out;
    yaw_ctrl->_last_rate_hp_in = rate_hp_in;

    //Calculate input to integrator
    float integ_in = - yaw_ctrl->_K_I * (yaw_ctrl->_K_A * accel_y + rate_hp_out);

    // Apply integrator, but clamp input to prevent control saturation and freeze integrator below min FBW speed
    // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
    // Don't integrate if _K_D is zero as integrator will keep winding up
    if (!disable_integrator && yaw_ctrl->_K_D > 0) {
        //only integrate if airspeed above min value
        if (aspeed > (float)aspd_min) {
            // prevent the integrator from increasing if surface defln demand is above the upper limit
            if (yaw_ctrl->_last_out < -45) {
                yaw_ctrl->_integrator += MAX(integ_in * delta_time, 0);
            } else if (yaw_ctrl->_last_out > 45) {
                // prevent the integrator from decreasing if surface defln demand  is below the lower limit
                yaw_ctrl->_integrator += MIN(integ_in * delta_time, 0);
            } else {
                yaw_ctrl->_integrator += integ_in * delta_time;
            }
        }
    } else {
        yaw_ctrl->_integrator = 0;
    }

    if (yaw_ctrl->_K_D < 0.0001f) {
        // yaw damping is disabled, and the integrator is scaled by damping, so return 0
        return 0;
    }

    // Scale the integration limit
    float intLimScaled = yaw_ctrl->_imax * 0.01f / (yaw_ctrl->_K_D * scaler * scaler);

    // Constrain the integrator state
    yaw_ctrl->_integrator = math_constrain_float(yaw_ctrl->_integrator, -intLimScaled, intLimScaled);

    // Protect against increases to _K_D during in-flight tuning from creating large control transients
    // due to stored integrator values
    if (yaw_ctrl->_K_D > yaw_ctrl->_K_D_last && yaw_ctrl->_K_D > 0) {
        yaw_ctrl->_integrator = yaw_ctrl->_K_D_last/yaw_ctrl->_K_D * yaw_ctrl->_integrator;
    }
    yaw_ctrl->_K_D_last = yaw_ctrl->_K_D;

    // Calculate demanded rudder deflection, +Ve deflection yaws nose right
    // Save to last value before application of limiter so that integrator limiting
    // can detect exceedance next frame
    // Scale using inverse dynamic pressure (1/V^2)
    yaw_ctrl->_pid_info.I = yaw_ctrl->_K_D * yaw_ctrl->_integrator * scaler * scaler;
    yaw_ctrl->_pid_info.D = yaw_ctrl->_K_D * (-rate_hp_out) * scaler * scaler;
    yaw_ctrl->_last_out =  yaw_ctrl->_pid_info.I + yaw_ctrl->_pid_info.D;

    // Convert to centi-degrees and constrain
    return math_constrain_float(yaw_ctrl->_last_out * 100, -4500, 4500);
}

// get actuator output for direct rate control
// desired_rate is in deg/sec. scaler is the surface speed scaler
float yctrl_get_rate_out(mp_yaw_controller_t yaw_ctrl, float desired_rate, float scaler, bool disable_integrator)
{
    const ahrs_view *_ahrs = get_ahrs_view();

    const float eas2tas = ahrs_get_EAS2TAS(_ahrs);
    bool limit_I = fabsf(yaw_ctrl->_last_out) >= 45 || disable_integrator;
    float rate_z = _ahrs->gyr.z;
    float aspeed;
    float old_I = pid_ctrl_get_i(&yaw_ctrl->rate_pid);

    if (!ahrs_airspeed_estimate(_ahrs, &aspeed)) {
        aspeed = 0;
    }
    bool underspeed = aspeed <= (float)yaw_ctrl->aparm->airspeed_min;
    if (underspeed) {
        limit_I = true;
    }

    // the P and I elements are scaled by sq(scaler). To use an
    // unmodified AC_PID object we scale the inputs and calculate FF separately
    //
    // note that we run AC_PID in radians so that the normal scaling
    // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
    pid_ctrl_update_all(&yaw_ctrl->rate_pid, radians(desired_rate) * scaler * scaler, rate_z * scaler * scaler, limit_I);

    if (underspeed) {
        // when underspeed we lock the integrator
        pid_ctrl_set_integrator1(&yaw_ctrl->rate_pid, old_I);
    }

    // FF should be scaled by scaler/eas2tas, but since we have scaled
    // the AC_PID target above by scaler*scaler we need to instead
    // divide by scaler*eas2tas to get the right scaling
    const float ff = degrees(pid_ctrl_get_ff(&yaw_ctrl->rate_pid) / (scaler * eas2tas));

    if (disable_integrator) {
        pid_ctrl_reset_i(&yaw_ctrl->rate_pid);
    }

    // convert AC_PID info object to same scale as old controller
    yaw_ctrl->_pid_info = yaw_ctrl->rate_pid._pid_info;

    const float deg_scale = degrees(1);
    yaw_ctrl->_pid_info.FF = ff;
    yaw_ctrl->_pid_info.P *= deg_scale;
    yaw_ctrl->_pid_info.I *= deg_scale;
    yaw_ctrl->_pid_info.D *= deg_scale;
    yaw_ctrl->_pid_info.limit = limit_I;

    // fix the logged target and actual values to not have the scalers applied
    yaw_ctrl->_pid_info.target = desired_rate;
    yaw_ctrl->_pid_info.actual = degrees(rate_z);

    // sum components
    float out = yaw_ctrl->_pid_info.FF + yaw_ctrl->_pid_info.P + yaw_ctrl->_pid_info.I + yaw_ctrl->_pid_info.D;

    // remember the last output to trigger the I limit
    yaw_ctrl->_last_out = out;

    if (yaw_ctrl->autotune != NULL && yaw_ctrl->autotune->running && aspeed > yaw_ctrl->aparm->airspeed_min) {
        // fake up an angular error based on a notional time constant of 0.5s
        const float angle_err_deg = desired_rate * yaw_ctrl->gains.tau;
        // let autotune have a go at the values
        mp_autotune_update(yaw_ctrl->autotune, &yaw_ctrl->_pid_info, scaler, angle_err_deg);
    }

    // output is scaled to notional centidegrees of deflection
    return math_constrain_float(out * 100, -4500, 4500);
}

void yctrl_reset_i(mp_yaw_controller_t yaw_ctrl)
{
    yaw_ctrl->_pid_info.I = 0;
    pid_ctrl_reset_i(&yaw_ctrl->rate_pid);
    yaw_ctrl->_integrator = 0;
}

void yctrl_reset_rate_pid(mp_yaw_controller_t yaw_ctrl)
{
    pid_ctrl_reset_i(&yaw_ctrl->rate_pid);
    pid_ctrl_reset_filter(&yaw_ctrl->rate_pid);
}

/*
  start an autotune
 */
void yctrl_autotune_start(mp_yaw_controller_t yaw_ctrl)
{
    if (yaw_ctrl->autotune == NULL && yctrl_rate_control_enabled(yaw_ctrl)) {
        // the autotuner needs a time constant. Use an assumed tconst of 0.5
        yaw_ctrl->gains.tau = 0.5f;
        yaw_ctrl->gains.rmax_pos = 90;

        yaw_ctrl->autotune = mp_autotune_probe(&yaw_ctrl->gains, MP_AUTOTUNE_YAW, yaw_ctrl->aparm, &yaw_ctrl->rate_pid, yaw_ctrl->_dt);
        if (yaw_ctrl->autotune == NULL) {
            if (!yaw_ctrl->failed_autotune_alloc) {
                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed yaw allocation");
            }
            yaw_ctrl->failed_autotune_alloc = true;
        }
    }
    if (yaw_ctrl->autotune != NULL) {
        mp_autotune_start(yaw_ctrl->autotune);
    }
}

/*
  restore autotune gains
 */
void yctrl_autotune_restore(mp_yaw_controller_t yaw_ctrl)
{
    if (yaw_ctrl->autotune != NULL) {
        mp_autotune_stop(yaw_ctrl->autotune);
    }
}

/**
  * @brief       
  * @param[in]   yaw_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
static void yctrl_assign_param(mp_yaw_controller_t yaw_ctrl)
{
    param_link_variable(PARAM_ID(YAW_CTRL, YAW2SRV_SLIP), &yaw_ctrl->_K_A);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW2SRV_INT), &yaw_ctrl->_K_I);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW2SRV_DAMP), &yaw_ctrl->_K_D);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW2SRV_RLL), &yaw_ctrl->_K_FF);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW2SRV_IMAX), &yaw_ctrl->_imax);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_ENABLE), &yaw_ctrl->_rate_enable);

    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_P), &yaw_ctrl->rate_pid._kp);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_I), &yaw_ctrl->rate_pid._ki);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_D), &yaw_ctrl->rate_pid._kd);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_FF), &yaw_ctrl->rate_pid._kff);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_IMAX), &yaw_ctrl->rate_pid._kimax);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_FLTT), &yaw_ctrl->rate_pid._filt_t_hz);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_FLTE), &yaw_ctrl->rate_pid._filt_e_hz);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_FLTD), &yaw_ctrl->rate_pid._filt_d_hz);
    param_link_variable(PARAM_ID(YAW_CTRL, YAW_RATE_SMAX), &yaw_ctrl->rate_pid._slew_rate_max);
}

/*------------------------------------test------------------------------------*/


